#include <iostream>
#include <conio.h>
#include "OpenGL.h"
#include "PhysicsBullet.h"
#include "PrecisionTimer.h"
#include "pgObjects.h"

#include "RagDoll.h"
#include "SpaceShip.h"

//#include "AntTweakBar.h"
//#include "TweakBar.h"

#include "btBulletDynamicsCommon.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"

//graphics headers
#include "g_renderer.h"
#include "g_playerShip.h"

using namespace std;

void myTickCallback(btDynamicsWorld *world, btScalar timeStep) {
    cout << "The world just ticked by %f seconds\n" << timeStep <<"\n";	
}

void drawCoordSystem(float len=100.0f)  {
	glBegin(GL_LINES);
	glColor3f(len, 0, 0);
	glVertex3d(0, 0, 0);
	glVertex3d(len, 0, 0);
	glColor3f(0, len, 0);
	glVertex3d(0, 0, 0);
	glVertex3d(0, len, 0);
	glColor3f(0, 0, len);
	glVertex3d(0, 0, 0);
	glVertex3d(0, 0, len);
	glEnd();

}

void clientResetScene(){
	//gNumClampedCcdMotions = 0;
	int numObjects = 0;
	int i;

	if (pgObjects::p_dynamicsWorld){
		numObjects = pgObjects::p_dynamicsWorld->getNumCollisionObjects();
	
		///create a copy of the array, not a reference!
		btCollisionObjectArray copyArray = pgObjects::p_dynamicsWorld->getCollisionObjectArray();

		for (i=0;i<numObjects;i++){
			btCollisionObject* colObj = copyArray[i];
			btRigidBody* body = btRigidBody::upcast(colObj);
			if (body){
				if (body->getMotionState()){
					btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
					myMotionState->m_graphicsWorldTrans = myMotionState->m_startWorldTrans;
					body->setCenterOfMassTransform( myMotionState->m_graphicsWorldTrans );
					colObj->setInterpolationWorldTransform( myMotionState->m_startWorldTrans );
					colObj->forceActivationState(ACTIVE_TAG);
					colObj->activate();
					colObj->setDeactivationTime(0);
					//colObj->setActivationState(WANTS_DEACTIVATION);
				}
				//removed cached contact points (this is not necessary if all objects have been removed from the dynamics world)
				if (pgObjects::p_dynamicsWorld->getBroadphase()->getOverlappingPairCache())
					pgObjects::p_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(colObj->getBroadphaseHandle(),pgObjects::p_dynamicsWorld->getDispatcher());

				btRigidBody* body = btRigidBody::upcast(colObj);
				if (body && !body->isStaticObject()){
					btRigidBody::upcast(colObj)->setLinearVelocity(btVector3(0,0,0));
					btRigidBody::upcast(colObj)->setAngularVelocity(btVector3(0,0,0));
				}
			}

		}

		///reset some internal cached data in the broadphase
		pgObjects::p_dynamicsWorld->getBroadphase()->resetPool(pgObjects::p_dynamicsWorld->getDispatcher());
		pgObjects::p_dynamicsWorld->getConstraintSolver()->reset();
	}

}

void main()
{
	//init Graphics
	renderer::initOpenGL();

	//init Audio
	//euthyme::initSound();

	//init Physics
	pgObjects::InitBulletPhysics();

	//init AI
	//callum::initAI() :P

	//FPS variables
	CPrecisionTimer fpsTimer;
	fpsTimer.Start();
	int frameNumber=0;
	double fpsTimeAccum=0.0f,refreshSince=1.0f,frameCorrection=1.0f/refreshSince;

	//ok taking the ground out for now...

	/*
	btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.),btScalar(10.),btScalar(200.)));
	pgObjects::p_collisionShapes.push_back(groundShape);
		btTransform groundTransform;
		groundTransform.setIdentity();
		groundTransform.setOrigin(btVector3(0,-10,0));
	btCollisionObject* fixedGround = new btCollisionObject();
		fixedGround->setCollisionShape(groundShape);
		fixedGround->setWorldTransform(groundTransform);
	pgObjects::p_dynamicsWorld->addCollisionObject(fixedGround);
	*/

	btVector3 aabbMin,aabbMax;
	aabbMin-=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
	aabbMax+=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
	btScalar m[16];
	btMatrix3x3 rot;

	PlayerShip* ship = new PlayerShip(btVector3(0,0,-100));
	ship->setObjectState(ACTIVE_STATE);

	while (renderer::RUNNING)
	{
		//FPS calculator Begin
		fpsTimeAccum+=fpsTimer.Stop();
		fpsTimer.Start();

		if(fpsTimeAccum>refreshSince)
		{
			fpsTimer.Fps = frameNumber*frameCorrection;
			frameNumber=0;
			fpsTimeAccum=0.0f;
		}

		frameNumber++;
		//FPS Calculator End

		//bullet physics: similar to integrate(duration);
		pgObjects::p_dynamicsWorld->stepSimulation(1/60.0f,10);
	
		rot.setIdentity();
		const int numObjects = pgObjects::p_dynamicsWorld->getNumCollisionObjects();		

		for(int i=0;i<numObjects;i++){
			btCollisionObject* colObj = pgObjects::p_dynamicsWorld->getCollisionObjectArray()[i];
			btRigidBody* body = btRigidBody::upcast(colObj);
			if(body && body->getMotionState()){
				btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
				myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(m);
				rot = myMotionState->m_graphicsWorldTrans.getBasis();
			}else{
				colObj->getWorldTransform().getOpenGLMatrix(m);
				rot = colObj->getWorldTransform().getBasis();
			}
			pgObjects::p_dynamicsWorld->getBroadphase()->getBroadphaseAabb(aabbMin,aabbMax);
		}

		// ----- rendering -----

		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
		glTranslatef(0, 0, -15);
		glRotatef(glfwGetTime() * 10, 0, 1, 0);

		pgObjects::renderScene();

		glfwSwapBuffers();

		// ----- rendering -----

		if (glfwGetKey(GLFW_KEY_ESC) || !glfwGetWindowParam(GLFW_OPENED)) renderer::RUNNING = false;
	}

	renderer::cleanup();

    //delete groundShape;

	Ragdoll::deleteRagdolls();
	pgObjects::ExitBulletPhysics();
	_getch();
}